function dz = springCartPoleDynamics(~,z,P) 
%DZ = SPRINGCARTPOLEDYNAMICS(T,Z,P)
% 
%FUNCTION:  This function computes the dynamics of a spring
%    cart-pole, and is designed to be called from ode45. The
%    model uses a point-mass cart and pendulum bob, with no
%    no friction or actuation
% 
%INPUTS: 
%    t = time. Dummy input for ode45. Not used.
%    z = [4xn] matrix of states.
%
%    P = struct of parameters
%OUTPUTS: 
%    dz = [4xn] matrix of state derivatives
% 
%NOTES:
%    This file was automatically generated by writeSpringCartPoleDynamics

m1 = P.m1; %cart mass
m2 = P.m2; %bob mass
g  = P.g ; %gravity
l = P.l; %pendulum length
k = P.k; %spring constant

x = z(1,:); %link one absolute angle
th = z(2,:); %link one angular rate
dx = z(3,:); %link two absolute angle
dth = z(4,:); %link two angular rate

f1 = dth.^2.*l.*m2.*sin(th) - k.*x;
f2 = -g.*l.*m2.*sin(th);

M11 = - m1 - m2;
M12 = -l.*m2.*cos(th);
M21 = -l.*m2.*cos(th);
M22 = -l.^2.*m2;

D = M11.*M22 - M12.*M21;

ddx = (f2.*M12 - f1.*M22)./D;
ddth = -(f2.*M11 - f1.*M21)./D;

dz = [...
    dx; %derivative of link one absolute angle
    dth; %derivative of link one angular rate
    ddx; %derivative of link two absolute angle
    ddth; %derivative of link two angular rate
];
end 
